/**
|--------------------------------Copyright-------------------------------------|
|                                                                              |
|                      (C)Copyright 2019,Chen Yikuang                          |
|                                                                              |
|------------------------------------------------------------------------------|
|  FileName     : Motor.c
|  Version      : v1.0
|  Author       : Chen Yikuang
|  Date         : 2019-03-23
|  Libsupports  : STM32F10x_StdPeriph_Lib_V3.5.0
|  Description  :
|------------------------------declaration of end------------------------------|
 **/
/*------------------------ I N C L U D E - F I L E S -------------------------*/
#include "app_motor.h"
/*----------------------------- D E F I N E S --------------------------------*/
/**/
/*------------ L O C A L - F U N C T I O N S - P R O T O T Y P E S -----------*/
/**/
/*--------------------- G L O B A L - F U N C T I O N S ----------------------*/
Motor_t Motor[1];
/*----------------------- L O C A L - F U N C T I O N S ----------------------*/
/**/
/*------------------------------- FILE OF END --------------------------------*/


/**
  * @Data   2019-05-23 12:46
  * @brief  电机参数初始化
  * @param  void
  * @retval void
  */
void Motor_PIDInit(void)
{

IncrementalPID_SetPara(&Motor[One].MotorPID,0.5, 0.05, 0.0,  PwmMaxOut,1000);
	
}

/**
  * @Data   2019-05-23 12:46
  * @brief  电机参数初始化
  * @param  void
  * @retval void
  */
void Motor_parmInit(void)
{
	Motor[One].ID=One;
	Motor[One].Mode=motor_Stop;
	Motor[One].GPIOx=MotorOne_GPIO;
	Motor[One].GPIOy=MotorOne_GPIO;
	Motor[One].GPIO_Pin_x=MotorOne_PIN_x;
	Motor[One].GPIO_Pin_y=MotorOne_PIN_y;
	Motor[One].MotorPID.Target = 0;
}
/**
  * @Data   2019-05-23 12:46
  * @brief  电机初始化
  * @param  void
  * @retval void
  */
void Motor_Init(void)
{
	//First step: GPIO_Init
	MOTOR_GPIO_Init();
	//Second step: PWM_Init
	Motor_PWM_Init();
	
	Motor_PIDInit();
	
	Motor_parmInit();
}

/**
  * @Data   2019-05-23 12:46
  * @brief  设置电机转向
  * @param  void
  * @retval void
  */
void Motor_setTurn(Motor_t* Motor,MotorMode_e turn)
{
	Motor->Mode = turn;
	switch (Motor->Mode)
  {
  	case motor_Stop:
			GPIO_WriteBit(Motor->GPIOx,Motor->GPIO_Pin_x,Bit_RESET);
			GPIO_WriteBit(Motor->GPIOy,Motor->GPIO_Pin_y,Bit_RESET);
  		break;
  	case motor_Clockwise:
			GPIO_WriteBit(Motor->GPIOx,Motor->GPIO_Pin_x,Bit_RESET);
			GPIO_WriteBit(Motor->GPIOy,Motor->GPIO_Pin_y,Bit_SET);
  		break;
  	case motor_Anticlockwise:
			GPIO_WriteBit(Motor->GPIOx,Motor->GPIO_Pin_x,Bit_SET);
			GPIO_WriteBit(Motor->GPIOy,Motor->GPIO_Pin_y,Bit_RESET);
  		break;
  }
}
/**
  * @Data   2019-05-23 12:46
  * @brief  设置电机pwm输出
  * @param  void
  * @retval void
  */
void Motor_pwmOut(Motor_t* Motor,int32_t PWM_Set)
{
		/*速度限幅*/
	if( PWM_Set > 0){
    if( PWM_Set > PwmMaxOut)
        PWM_Set = PwmMaxOut;
	}else{
    if( PWM_Set < -PwmMaxOut)
        PWM_Set = -PwmMaxOut;
	}
	
	if(Motor->ID==One){
		if(Motor->Mode == motor_Anticlockwise){
				TIM1->CCR3 = PWM_Set;
				TIM1->CCR4 = 0; 
		}
		else if(Motor->Mode == motor_Clockwise){
				TIM1->CCR3 = 0;
				TIM1->CCR4 = PWM_Set; 
		}
		else if(Motor->Mode == motor_Stop){
				TIM1->CCR3 = 0;
				TIM1->CCR4 = 0; 
		}
	}	
}
/**
  * @Data   2019-05-23 12:46
  * @brief  设置电机速度
  * @param  void
  * @retval void
  */
void Motor_setSpeed(Motor_t* Motor,int32_t MotorSpeed)
{
	int32_t PWM_Set;
	
	Motor_pwmOut(Motor,0);
	Motor_setTurn(Motor, motor_Stop);
#if !USE_FreeRTOS
	delay_ms(1);
#endif
#if USE_FreeRTOS
	delay_xms(1);
#endif
	
	/*速度限幅*/
	if( MotorSpeed > 0){
    if( MotorSpeed > PwmMaxOut)
        MotorSpeed = PwmMaxOut;
	}else{
    if( MotorSpeed < -PwmMaxOut)
        MotorSpeed = -PwmMaxOut;
	}
	
  if(MotorSpeed > 0)
	{
		PWM_Set = MotorSpeed;
		Motor_setTurn(Motor, motor_Anticlockwise);
		Motor_pwmOut(Motor,PWM_Set);
	}
	else if(MotorSpeed < 0)
	{
		PWM_Set = 0-MotorSpeed;
		Motor_setTurn(Motor, motor_Clockwise);
		Motor_pwmOut(Motor,PWM_Set);
	}
	else if(MotorSpeed == 0)
	{
		PWM_Set = 0;
		Motor_setTurn(Motor, motor_Stop);
		Motor_pwmOut(Motor,PWM_Set);
	}
}

/**
* @brief	限制最大输出
* @param [in] 输入值
*				 [in] 限制的最大值
* @retval None
*/
void abs_limit_i(int *a, int ABS_MAX)
{
	if(*a > 0){
    if(*a > ABS_MAX)
        *a = ABS_MAX;
	}else{
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
	}
}




